/*
 * Gyro.h
 *
 * Created: 1/19/2013 7:11:02 PM
 *  Author: Alexander
 */ 


#ifndef GYRO_H_
#define GYRO_H_

/* Includes ------------------------------------------------------------------*/

#include <avr/io.h>
#include <stdint.h>
#include <math.h>
#include "SPI.h"
#include "Vector_Class.h"

#ifndef __SHARED__TYPES
#define __SHARED__TYPES

typedef enum {
	MEMS_SUCCESS	=		0x01,
	MEMS_ERROR		=		0x00
} status_t;

typedef enum {
	MEMS_ENABLE		=		0x01,
	MEMS_DISABLE	=		0x00
} State_t;

#endif /*__SHARED__TYPES*/

/*********************DATA RATES AND BANDWIDTHS*********************/
	#define	L3GD20_ODR_95Hz_BW_12_5		0x00
	#define L3GD20_ODR_95Hz_BW_25		0x01
	#define L3GD20_ODR_190Hz_BW_12_5	0x04
	#define L3GD20_ODR_190Hz_BW_25		0x05
	#define L3GD20_ODR_190Hz_BW_50		0x06
	#define L3GD20_ODR_190Hz_BW_70		0x07
	#define L3GD20_ODR_380Hz_BW_20		0x08
	#define L3GD20_ODR_380Hz_BW_25		0x09
	#define L3GD20_ODR_380Hz_BW_50		0x0A
	#define L3GD20_ODR_380Hz_BW_110		0x0B
	#define L3GD20_ODR_760Hz_BW_30		0x0C
	#define L3GD20_ODR_760Hz_BW_35		0x0D
	#define L3GD20_ODR_760Hz_BW_50		0x0E
	#define L3GD20_ODR_760Hz_BW_110		0x0F

/*********************OPERATING MODES*********************/
	#define L3GD20_POWER_DOWN	0x00
	#define L3GD20_SLEEP		0x01
	#define L3GD20_NORMAL		0x01

/*********************HIGH POWER MODES*********************/
	#define L3GD20_HPM_NORMAL_MODE_RES  0x00
	#define L3GD20_HPM_REF_SIGNAL       0x01
	#define L3GD20_HPM_NORMAL_MODE      0x02
	#define L3GD20_HPM_AUTORESET_INT    0x03

/*********************CUT OFF FREQUENCY*********************/
	#define L3GD20_HPFCF_0	0x00
	#define L3GD20_HPFCF_1	0x01
	#define L3GD20_HPFCF_2	0x02
	#define L3GD20_HPFCF_3	0x03
	#define L3GD20_HPFCF_4	0x04
	#define L3GD20_HPFCF_5	0x05
	#define L3GD20_HPFCF_6	0x06
	#define L3GD20_HPFCF_7	0x07
	#define L3GD20_HPFCF_8	0x08
	#define L3GD20_HPFCF_9	0x09

/*********************INTERRUPT PIN MODE*********************/
	#define L3GD20_PUSH_PULL	0x00
	#define L3GD20_OPEN_DRAIN	0x01
	
/*********************FULLSCALE*********************/
	#define L3GD20_FULLSCALE_250	0x00
	#define L3GD20_FULLSCALE_500	0x01
	#define L3GD20_FULLSCALE_2000	0x02

/*********************ENDIANESS*********************/
	#define L3GD20_BLE_LSB	0x00
	#define L3GD20_BLE_MSB	0x01

/*********************SPI MODE*********************/
	#define L3GD20_SPI_4_WIRE	0x00
	#define L3GD20_SPI_3_WIRE	0x01

/*********************FIFO MODE*********************/
	#define L3GD20_FIFO_DISABLE					0x05
	#define L3GD20_FIFO_BYPASS_MODE				0x00
	#define L3GD20_FIFO_MODE					0x01
	#define L3GD20_FIFO_STREAM_MODE				0x02
	#define L3GD20_FIFO_STREAM_TO_FIFO_MODE		0x03
	#define L3GD20_FIFO_BYPASS_TO_STREAM_MODE	0x04
	
/*********************HIGH PASS FILTER OPERATION*********************/
	#define L3GD20_NONE				0x00
	#define L3GD20_HPF				0x01
	#define L3GD20_LPF2				0x02
	#define L3GD20_HPFLPF2			0x03

/* Exported macro ------------------------------------------------------------*/

#ifndef __SHARED__MACROS

#define __SHARED__MACROS
#define ValBit(VAR,Place)  (VAR & (1<<Place))
#define BIT(x) ( (x) )

#endif /*__SHARED__MACROS*/

/* Exported constants --------------------------------------------------------*/

#ifndef __SHARED__CONSTANTS
#define __SHARED__CONSTANTS

#define MEMS_SET                    0x01
#define MEMS_RESET                  0x00

#endif /*__SHARED__CONSTANTS*/

/**************CONTROL REGISTERS*****************/

#define L3GD20_MEMS_I2C_ADDRESS     0xD6

/***************CTRL1***************/
#define L3GD20_CTRL_REG1			0x20
#define L3GD20_ODR_BIT              BIT(4)
#define L3GD20_PD					BIT(3)
#define L3GD20_ZEN					BIT(2)
#define L3GD20_YEN					BIT(1)
#define L3GD20_XEN					BIT(0)
#define L3GD20_X_ENABLE             0x02
#define L3GD20_Y_ENABLE             0x01
#define L3GD20_Z_ENABLE             0x04

/***************CTRL2***************/
#define L3GD20_CTRL_REG2				0x21
#define L3GD20_HPM						BIT(4)
#define L3GD20_HPFC3					BIT(3)
#define L3GD20_HPFC2					BIT(2)
#define L3GD20_HPFC1					BIT(1)
#define L3GD20_HPFC0					BIT(0)

/***************CTRL3***************/
#define L3GD20_CTRL_REG3				0x22
#define L3GD20_I1_INT					BIT(7)
#define L3GD20_I1_BOOT					BIT(6)
#define L3GD20_H_LACTIVE				BIT(5)
#define L3GD20_PP_OD					BIT(4)
#define L3GD20_I2_DRDY					BIT(3)
#define L3GD20_I2_WTM					BIT(2)
#define L3GD20_I2_ORUN					BIT(1)
#define L3GD20_I2_EMPTY					BIT(0)
#define L3GD20_I1_ON_PIN_INT1_ENABLE                   0x80
#define L3GD20_I1_ON_PIN_INT1_DISABLE                  0x00
#define L3GD20_I1_BOOT_ON_INT1_ENABLE                  0x40
#define L3GD20_I1_BOOT_ON_INT1_DISABLE                 0x00
#define L3GD20_INT1_ACTIVE_HIGH                        0x00
#define L3GD20_INT1_ACTIVE_LOW                         0x20
#define L3GD20_I2_DRDY_ON_INT2_ENABLE                  0x08
#define L3GD20_I2_DRDY_ON_INT2_DISABLE                 0x00
#define L3GD20_WTM_ON_INT2_ENABLE                      0x04
#define L3GD20_WTM_ON_INT2_DISABLE                     0x00
#define L3GD20_OVERRUN_ON_INT2_ENABLE                  0x02
#define L3GD20_OVERRUN_ON_INT2_DISABLE                 0x00
#define L3GD20_EMPTY_ON_INT2_ENABLE                    0x01
#define L3GD20_EMPTY_ON_INT2_DISABLE                   0x00

/***************CTRL4***************/
#define L3GD20_CTRL_REG4				0x23
#define L3GD20_BDU						BIT(7)
#define L3GD20_BLE						BIT(6)
#define L3GD20_FS						BIT(4)
#define L3GD20_SIM						BIT(0)

/***************CTRL5***************/
#define L3GD20_CTRL_REG5			        0x24
#define L3GD20_FIFO_EN                      BIT(6)
#define L3GD20_HPEN                         BIT(4)
#define L3GD20_INT1_SEL1                    BIT(3)
#define L3GD20_INT1_SEL0                    BIT(2)
#define L3GD20_OUT_SEL1                     BIT(1)
#define L3GD20_OUT_SEL0                     BIT(0)

/**************GYROSCOPE INTERRUPT REGISTERS***************/
#define L3GD20_INT1_CFG					0x30
#define L3GD20_INT1_SRC					0x31
#define L3GD20_INT1_THS_XH				0x32
#define L3GD20_INT1_THS_XL				0x33
#define L3GD20_INT1_THS_YH				0x34
#define L3GD20_INT1_THS_YL				0x35
#define L3GD20_INT1_THS_ZH				0x36
#define L3GD20_INT1_THS_ZL				0x37
#define L3GD20_INT1_DURATION			0x38
#define L3GD20_LIR                      BIT(6)
#define L3GD20_ANDOR                    BIT(7)
#define L3GD20_ZHIE                     BIT(5)
#define L3GD20_ZLIE                     BIT(4)
#define L3GD20_YHIE                     BIT(3)
#define L3GD20_YLIE                     BIT(2)
#define L3GD20_XHIE                     BIT(1)
#define L3GD20_XLIE                     BIT(0)
#define L3GD20_INT1_AND                 0x80
#define L3GD20_INT1_OR                  0x00
#define L3GD20_INT1_LIR_ENABLE          0x40
#define L3GD20_INT1_LIR_DISABLE         0x00
#define L3GD20_INT1_ZHIE_ENABLE         0x20
#define L3GD20_INT1_ZHIE_DISABLE        0x00
#define L3GD20_INT1_ZLIE_ENABLE         0x10
#define L3GD20_INT1_ZLIE_DISABLE        0x00
#define L3GD20_INT1_YHIE_ENABLE         0x08
#define L3GD20_INT1_YHIE_DISABLE        0x00
#define L3GD20_INT1_YLIE_ENABLE         0x04
#define L3GD20_INT1_YLIE_DISABLE        0x00
#define L3GD20_INT1_XHIE_ENABLE         0x02
#define L3GD20_INT1_XHIE_DISABLE        0x00
#define L3GD20_INT1_XLIE_ENABLE         0x01
#define L3GD20_INT1_XLIE_DISABLE        0x00

/**********GYROSCOPE: STATUS AND OUTPUT REGISTERS***********/
//OUTPUT REGISTER
#define L3GD20_OUT_X_L					0x28
#define L3GD20_OUT_X_H					0x29
#define L3GD20_OUT_Y_L					0x2A
#define L3GD20_OUT_Y_H					0x2B
#define L3GD20_OUT_Z_L					0x2C
#define L3GD20_OUT_Z_H					0x2D
#define L3GD20_STATUS_REG               0x27
#define L3GD20_STATUS_REG_ZYXOR         0x07    // 1	:	new data set has over written the previous one
												// 0	:	no overrun has occurred (default)
#define L3GD20_STATUS_REG_ZOR           0x06    // 0	:	no overrun has occurred (default)
												// 1	:	new Z-axis data has over written the previous one
#define L3GD20_STATUS_REG_YOR           0x05    // 0	:	no overrun has occurred (default)
												// 1	:	new Y-axis data has over written the previous one
#define L3GD20_STATUS_REG_XOR           0x04    // 0	:	no overrun has occurred (default)
												// 1	:	new X-axis data has over written the previous one
#define L3GD20_STATUS_REG_ZYXDA         0x03    // 0	:	a new set of data is not yet avvious one
												// 1	:	a new set of data is available
#define L3GD20_STATUS_REG_ZDA           0x02    // 0	:	a new data for the Z-Axis is not availvious one
												// 1	:	a new data for the Z-Axis is available
#define L3GD20_STATUS_REG_YDA           0x01    // 0	:	a new data for the Y-Axis is not available
												// 1	:	a new data for the Y-Axis is available
#define STATUS_REG_XDA                  0x00    // 0	:	a new data for the X-Axis is not available

#define L3GD20_DATAREADY_BIT            L3GD20_STATUS_REG_ZYXDA

#define L3GD20_I_AM_L3GD20			    0xD4

/*************GYROSCOPE FIFO CONTROL REGISTER**************/
#define L3GD20_FM0                      BIT(5)
#define L3GD20_FIFO_CTRL_REG            0x2E
#define L3GD20_FIFO_SRC_REG				0x2F

/***********************READ FUNCTIONS*********************/
uint8_t L3GD20_ReadReg(uint8_t Reg, uint8_t* Data);
uint8_t L3GD20_Read_Vectors_All(AxesVector_t* Data);
uint8_t L3GD20_WriteReg(uint8_t Reg, uint8_t Data);

/*********************INITIALIZE FUNCTION*********************/
void Gyro_Init(void);
AxesVector_ptr Zero_Rate_Calc();
void Calculate_Angle_Rate(AxesVector_t *vector_raw, AxesVector_t *zero_rate_vector);
void Calculate_Angle(AxesVector_t* zero_rate_vector, AxesVector_t* vector, AxesVector_t* angle);
#endif /* GYRO_H_ */

